import subprocess
import sys
import logging
from typing import List, Optional, Tuple

# 配置日志
logging.basicConfig(
    level=logging.INFO,
    format='%(asctime)s - %(levelname)s - %(message)s',
    handlers=[
        logging.FileHandler("robot_control.log"),
        logging.StreamHandler(sys.stdout)
    ]
)
logger = logging.getLogger(__name__)

class RobotController:
    """机器人控制客户端类"""
    
    def __init__(self, binary_path: str = "/home/unitree/softwares/unitree-sdk2-project/build/bin/g1_loco_client"):
        """
        初始化机器人控制器
        
        Args:
            binary_path: 二进制文件的路径
        """
        self.binary_path = binary_path
        self.process: Optional[subprocess.Popen] = None
        
    def send_velocity_command(self, 
                             network_interface: str = "eth0", 
                             x: float = 0.0, 
                             y: float = 0.0, 
                             z: float = 0.0, 
                             w: float = 0.0,
                             timeout: int = 10) -> Tuple[bool, str]:
        """
        发送速度控制命令到机器人
        
        Args:
            network_interface: 网络接口名称
            x: x方向速度 (m/s)
            y: y方向速度 (m/s)
            z: z方向速度 (m/s)
            w: 角速度 (rad/s)
            timeout: 命令超时时间(秒)
            
        Returns:
            (成功标志, 输出信息)
        """
        # 构建命令
        velocity_str = f'"{x} {y} {z} {w}"'
        cmd = [
            self.binary_path,
            f"--network_interface={network_interface}",
            f"--set_velocity={velocity_str}"
        ]
        
        logger.info(f"执行命令: {' '.join(cmd)}")
        
        try:
            # 执行命令
            self.process = subprocess.Popen(
                cmd,
                stdout=subprocess.PIPE,
                stderr=subprocess.PIPE,
                text=True,
                encoding='utf-8'
            )
            
            # 等待命令完成或超时
            stdout, stderr = self.process.communicate(timeout=timeout)
            return_code = self.process.returncode
            
            if return_code == 0:
                logger.info(f"命令执行成功: {stdout}")
                return True, stdout
            else:
                logger.error(f"命令执行失败 (返回码: {return_code}): {stderr}")
                return False, stderr
                
        except subprocess.TimeoutExpired:
            logger.error("命令执行超时")
            if self.process:
                self.process.kill()
                stdout, stderr = self.process.communicate()
            return False, "命令执行超时"
            
        except FileNotFoundError:
            error_msg = f"找不到二进制文件: {self.binary_path}"
            logger.error(error_msg)
            return False, error_msg
            
        except Exception as e:
            error_msg = f"执行命令时发生未知错误: {str(e)}"
            logger.error(error_msg)
            return False, error_msg
            
    def stop(self):
        """停止当前进程"""
        if self.process and self.process.poll() is None:
            self.process.terminate()
            try:
                self.process.wait(timeout=5)
            except subprocess.TimeoutExpired:
                self.process.kill()
            logger.info("已停止机器人控制进程")

# 使用示例
if __name__ == "__main__":
    # 创建机器人控制器实例-默认参数binary_path
    controller = RobotController()
    
    # 示例1: 发送速度命令
    success, message = controller.send_velocity_command(
        network_interface="eth0",
        x=0.2,
        y=0.0,
        z=0.0,
        w=1.0
    )
    
    if success:
        print("✅ 命令执行成功!")
        print(f"输出: {message}")
    else:
        print("❌ 命令执行失败!")
        print(f"错误: {message}")
        sys.exit(1)
    
    # # 示例2: 发送另一个速度命令
    # print("\n发送另一个命令...")
    # success, message = controller.send_velocity_command(
    #     network_interface="eth0",
    #     x=0.0,  # 停止
    #     y=0.0,
    #     z=0.0,
    #     w=0.0
    # )
    
    # if success:
    #     print("✅ 停止命令执行成功!")
    # else:
    #     print("❌ 停止命令执行失败!")